/********************************************************************************
* ADXL345 Library - ADXL345.cpp                                                 *
*                                                                               *
* Copyright (C) 2012 Anil Motilal Mahtani Mirchandani(anil.mmm@gmail.com)       *
*                                                                               *
* License GPLv3+: GNU GPL version 3 or later <http://gnu.org/licenses/gpl.html> *
* This is free software: you are free to change and redistribute it.            *
* There is NO WARRANTY, to the extent permitted by law.                         *
*                                                                               *
*********************************************************************************/

#include <ADXL345.h>
#include <Wire.h>


byte vtbuffer[6];
byte zG[3];

double fXg;
double fYg;
double fZg;

const float alpha = 0.5;

void ADXL345::writeRegister(byte reg_addr, int nbytes, byte *buffer)
{
	Wire.beginTransmission(ADXL345_ADDRESS);
		
	//Wire.write(reg_addr);
	Wire.send(reg_addr);
	
	//written_bytes = Wire.write(buffer, nbytes);
	Wire.send(buffer, nbytes);
	
	Wire.endTransmission();
}

int ADXL345::readRegister(byte reg_addr, int nbytes, byte *buffer)
{
	int idx = 0;

	Wire.beginTransmission(ADXL345_ADDRESS);

	//Wire.write(reg_addr);
	Wire.send(reg_addr);
	
	Wire.endTransmission(); 

	Wire.requestFrom(ADXL345_ADDRESS, nbytes);

	while(Wire.available() && idx < nbytes)
	{ 
		//buffer[idx++] = Wire.read();
	  buffer[idx++] = Wire.receive();	
	}

	return idx;
}

ADXL345::ADXL345()
{
	Wire.begin();

	zG[0] = -20;
	zG[1] =  15;
	zG[2] = -23;
	
    fXg = 0;
    fYg = 0;
    fZg = 0;	
}

void ADXL345::begin()
{
	byte data = 0x08;
	writeRegister(ADXL345_POWER_CTL, 1, &data);
}

void ADXL345::end()
{
	byte data = 0x00;
	writeRegister(ADXL345_POWER_CTL, 1, &data);
}

 void ADXL345::read(double *x, double *y, double *z)
 {
	 byte buffer[6];

	 readRegister(ADXL345_DATAX0, 6, buffer);

	 *x = ((buffer[0] + (buffer[1] << 8)) - zG[0])/256.0;
	 *y = ((buffer[2] + (buffer[3] << 8)) - zG[1])/256.0;
	 *z = ((buffer[4] + (buffer[5] << 8)) - zG[2])/256.0;
}

// void ADXL345::read(int *x, int *y, int *z)
// {
	// byte buffer[6];

	// readRegister(ADXL345_DATAX0, 6, buffer);

	// *x = buffer[0] + (buffer[1] << 8);
	// *y = buffer[2] + (buffer[3] << 8);
	// *z = buffer[4] + (buffer[5] << 8);
// }

int ADXL345::getRawX()
{
  return vtbuffer[0] + (vtbuffer[1] << 8);
}

int ADXL345::getRawY()
{
  return vtbuffer[2] + (vtbuffer[3] << 8);
}

int ADXL345::getRawZ()
{
  return vtbuffer[4] + (vtbuffer[5] << 8);
}

double ADXL345::ADXL345::getGX()
{
  return (getRawX() - zG[0]) / 256.0; 
}

double ADXL345::getGY()
{
  return (getRawY() - zG[1]) / 256.0; 
}

double ADXL345::getGZ()
{
  return (getRawZ() - zG[2]) / 256.0; 
}

void ADXL345::read()
 {
   readRegister(ADXL345_DATAX0, 6, vtbuffer);
 }

double ADXL345::getRoll()
{
  return (atan2(-fYg, fZg) * 180.0) / M_PI;
}

double ADXL345::getPitch()
{
  return (atan2(fXg, sqrt(fYg * fYg + fZg * fZg)) * 180.0) / M_PI; 
} 

void ADXL345::processLPFilter()
{
  //Low Pass Filter
  fXg = getGX() * alpha + (fXg * (1.0 - alpha));
  fYg = getGY() * alpha + (fYg * (1.0 - alpha));
  fZg = getGZ() * alpha + (fZg * (1.0 - alpha));
}
 
void ADXL345::setZeroG(double x, double y, double z)
{
	zG[0] = x * 256.0;
	zG[1] = y * 256.0;
	zG[2] = z * 256.0;
}

void ADXL345::setZeroG(int x, int y, int z)
{
	zG[0] = x;
	zG[1] = y;
	zG[2] = z;
}
